#ifndef __FEEDFORWARDPID_H
#define __FEEDFORWARDPID_H

#define SELECT 0 /*位置式：0，增量式：1*/

#if SELECT

/*增量式PID*/
#define KP1 2.25f // P参数
#define KI1 0.00f // I参数
#define KD1 0.0f  // D参数

#define KP2 6.25f    // P参数
#define KI2 0.00750f // I参数
#define KD2 0.0f     // D参数

#define SAMPL_TIM 5 // 采样时间 单位ms

#else
/*位置式PID*/

/*电机1的PID*/
#define Speed_KP1 25.0f // P参数
#define Speed_KI1 8.2f  // I参数
#define Speed_KD1 0.0f  // D参数

#define Angle_KP1 6.0f   // P参数
#define Angle_KI1 0.16f  // I参数
#define Angle_KD1 16.0f  // D参数

#define Angle_Kv1 0.015f // 速度前馈增益
#define Angle_Ka1 0.0f   // 加速度前馈增益

/*电机2的PID*/

#define Speed_KP2 24.0f // P参数
#define Speed_KI2 7.5f  // I参数
#define Speed_KD2 0.0f  // D参数

#define Angle_KP2 5.8f   // P参数
#define Angle_KI2 0.000f // I参数
#define Angle_KD2 48.0f   // D参数

#define Angle_Kv2 0.015f // 速度前馈增益
#define Angle_Ka2 0.0f // 加速度前馈增益

#define SAMPL_TIM 5 // 采样时间 单位ms

#endif

/*PID结构体*/
typedef struct
{
    float Setpoint;    /*设定目标*/
    float ActualValue; /*期望输出值*/
    float SumError;    /*误差累计*/
    float Proportion;  /*比例常数P*/
    float integral;    /*积分常数I*/
    float Derivation;  /*微分常数D*/
    float Error;       /*e[n]*/
    float LastError;   /*e[n-1]*/
    float PrevError;   /*e[n-2]*/

    float Pidoutput; /*传统PID的输出*/

    // 新增前馈参数
    float Kv;           // 速度前馈增益
    float Ka;           // 加速度前馈增益
    float LastSetpoint; // 上一次目标位置
    float PrevSetpoint; // 上上次目标位置
    uint32_t last_time; // 上次计算时间戳（新增）

} FeedForwardPID_TypeDef;

void PID_Init(void); // PID初始化

float Pid_closed_control(FeedForwardPID_TypeDef *PID, float Feedback_value);
float FeedForwardPid_closed_control(FeedForwardPID_TypeDef *PID, float Feedback_value); // Feedback_value实际值

#endif
